#ifndef __TEST_GLOBAL_PLANNER_H_
#define __TEST_GLOBAL_PLANNER_H_

#include <ros/ros.h>
#include "../core/core.h"
#include "../path_planner/global_planner/global_planner_base.h"
#include "../path_planner/global_planner/a_star/a_star.h"

class GlobalPlanner{
    // 定义路径规划函数指针
    // void(GlobalPlannerBase::*PlannerPtr)() const;

    public:
        GlobalPlanner();
        ~GlobalPlanner();

        // 发布原始路径
        void PublishPath(ros::Publisher& pub, vector<Pose2D>& points);
        // 发布扩展点
        void PublishExpendPointsMap(vector<Pixel2D>& expend_points);

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGridConstPtr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);

    private:
        ros::NodeHandle __nh;
        ros::Publisher __path_pub, __path_smooth_pub;
        ros::Publisher __start_pose_pub;
        ros::Publisher __goal_pose_pub;
        ros::Publisher __expand_points_pub;
        ros::Subscriber __map_sub;
        ros::Subscriber __rviz_pose_sub;

    private:
        boost::shared_ptr<const nav_msgs::OccupancyGrid> __map_ptr;
        vector<geometry_msgs::PoseStamped> __rviz_poses;
        GlobalPlannerType __global_planner_type;    //规划器类型
        Map2D __map_2d;      
        boost::shared_ptr<GlobalPlannerBase> __planner_ptr;
        
};

#endif

